package physics.MovementControl;

import physics.GameObject;

import common.Math3D;
import common.Point3D;
import common.Position3D;


public class AbstractMovementController {

	private static final double movementEpsilon = 0.00001;
	private static final Point3D LEFT = new Point3D(1,0,0);
	private static final Point3D UP = new Point3D(0,1,0);
	private static final Point3D FACE = new Point3D(0,0,1);
	
	Point3D movementMomentum = new Point3D(0,0,0);
	Point3D rotationMomentumLeft = new Point3D(1,0,0);;
	Point3D rotationMomentumUp = new Point3D(0,1,0);;
	Point3D rotationMomentumFace = new Point3D(0,0,1);;
	Position3D lastPosition = new Position3D();
	
	double movementFriction = 0;
	double rotationFriction = 0;
	double movementLimit = 1;
	double rotationLimit = 0.1;

	boolean hasMomentum = false;

	GameObject object;
	
	
	double distance = 0;

	public AbstractMovementController(GameObject object){
		this.object = object;
	}
	public void play(){
		storeLastPosition();
		limitMovement();
		limitRotation();
		if (movementMomentum.norm() > movementEpsilon)
			doMoveMomentum();
		doRotationMomentum();
		updateMomentum();
		updateDistancePassed();
	}
	private void updateDistancePassed() {
		distance = Math3D.getDistance(lastPosition.getCenter(), object.getCenter());
	}

	public void setMovement(Point3D movement){
		this.movementMomentum = movement; 
		limitMovement();
	}

	public void addMovement(Point3D movement){
		this.movementMomentum.add(movement);
		limitMovement();
	}

	public void setRotation(Point3D left, Point3D up, Point3D face){
		this.rotationMomentumFace = face;
		this.rotationMomentumLeft = left;
		this.rotationMomentumUp = up;
	}
	public void addRotation(Point3D left, Point3D up, Point3D face){
		this.rotationMomentumFace.rotateWith(left, up, face);
		this.rotationMomentumUp.rotateWith(left, up, face);
		this.rotationMomentumLeft.rotateWith(left, up, face);
	}

	public void rotationY(double angle){
		Point3D upDateFace = new Point3D(Math.sin(angle),0,Math.cos(angle));
		Point3D upDateLeft = new Point3D(Math.cos(angle),0,-Math.sin(angle));
		Point3D upDateUp = new Point3D(0,1,0);
		
		setRotation(upDateLeft, upDateUp, upDateFace);
	}

	public void addRotationY(double angle) {
		Point3D upDateFace = new Point3D(Math.sin(angle),0,Math.cos(angle));
		Point3D upDateLeft = new Point3D(Math.cos(angle),0,-Math.sin(angle));
		Point3D upDateUp = new Point3D(0,1,0);
		addRotation(upDateLeft, upDateUp, upDateFace);
	}


	public void rotationX(double angle){
		Point3D upDateFace = new Point3D(0,Math.sin(angle),Math.cos(angle));
		Point3D upDateUp = new Point3D(0,Math.cos(angle),-Math.sin(angle));
		Point3D upDateLeft = new Point3D(1,0,0);
		setRotation(upDateLeft, upDateUp, upDateFace);
	}

	public void addRotationX(double angle) {
		Point3D upDateFace = new Point3D(0,Math.sin(angle),Math.cos(angle));
		Point3D upDateUp = new Point3D(0,Math.cos(angle),-Math.sin(angle));
		Point3D upDateLeft = new Point3D(1,0,0);
		addRotation(upDateLeft, upDateUp, upDateFace);
	}

	protected void doMovementFriction(){
		if (movementFriction <= 0) return;

		double reducedSpeed = this.movementMomentum.norm() - movementFriction;
		if (reducedSpeed <= movementEpsilon){
			movementMomentum.setValues(0, 0, 0);
		}else{
			movementMomentum.normalize(reducedSpeed);
		}

	}
	protected void doRotationFriction(){
		if (rotationFriction <= 0) return;
		frict_Vector(rotationMomentumLeft, LEFT);
		frict_Vector(rotationMomentumUp, UP);
		frict_Vector(rotationMomentumFace, FACE);
	}
	

	protected void doMoveMomentum(){
		this.object.getCenter().add(movementMomentum);
	}

	protected void doRotationMomentum(){
		//Position3D position = object.getPosition();
		Point3D face = rotationMomentumFace.clone();
		Point3D up = rotationMomentumUp.clone();
		Point3D left = rotationMomentumLeft.clone();
		
		face.rotateWith(object.getLeft(), object.getUp(), object.getFace());
		up.rotateWith(object.getLeft(), object.getUp(), object.getFace());
		left.rotateWith(object.getLeft(), object.getUp(), object.getFace());
		
		object.getLeft().copy(left);
		object.getUp().copy(up);
		object.getFace().copy(face);
	}

	protected void resetMomentum(){
		this.movementMomentum.setValues(0, 0, 0);
		this.rotationMomentumFace.setValues(0, 0, 1);
		this.rotationMomentumUp.setValues(0, 1, 0);
		this.rotationMomentumLeft.setValues(1, 0, 0);
	}

	protected void updateMomentum() {
		if (hasMomentum){
			doMovementFriction();
			doRotationFriction();
		}else{
			resetMomentum();
		}
	}

	protected void limitMovement() {
		if (movementLimit >= 0 && movementMomentum.norm() > movementLimit){
			movementMomentum.normalize(movementLimit);
		}
	}

	protected void limitRotation() {
		if (rotationLimit <= 0) return;
		limit_Vector(rotationMomentumLeft, LEFT);
		limit_Vector(rotationMomentumUp, UP);
		limit_Vector(rotationMomentumFace, FACE);

	}

	private void frict_Vector(Point3D v, Point3D s){
		double alpha = Math.acos(Math3D.getDotProduct(v, s));
		double friction = rotationFriction;
		
		if (alpha <= rotationFriction){
			v.copy(s);
			return;
		}
		
		if (alpha > rotationLimit){
			friction += alpha - rotationLimit;
		}
		
		if (friction > alpha){
			friction = alpha;
		}
		
		Point3D difvec = Math3D.subPoint(s, v);
		double difscale =  friction / alpha;
		difvec.scale(difscale);
		v.add(difvec);
		v.normalize();
	}
	

	private void limit_Vector(Point3D v, Point3D s){
		double alpha = Math.acos(Math3D.getDotProduct(v, s));
		double friction = 0;
		
		if (alpha > rotationLimit){
			friction += alpha - rotationLimit;
		}
		else return;
		
		Point3D difvec = Math3D.subPoint(s, v);
		double difscale =  friction / alpha;
		difvec.scale(difscale);
		v.add(difvec);
		v.normalize();
	}

	public void restoreLastPosition(){
		object.getPosition().copy(lastPosition);
	}
	
	private void storeLastPosition(){
		this.lastPosition.copy(object.getPosition());
	}

	
	public boolean inTheAir(){
		return (object.getPlanet().surfaceDistance(object) - 0.001 > object.getRadius());
	}

	public boolean hasMomentum() {
		return hasMomentum;
	}
	public void setHasMomentum(boolean hasMomentum) {
		this.hasMomentum = hasMomentum;
	}
	public double getMovementLimit() {
		return movementLimit;
	}
	public void setMovementLimit(double movementLimit) {
		this.movementLimit = movementLimit;
	}
	public double getRotationLimit() {
		return rotationLimit;
	}
	public void setRotationLimit(double rotationLimit) {
		this.rotationLimit = rotationLimit;
	}
	
	public double getMovementFriction() {
		return movementFriction;
	}
	public void setMovementFriction(double movementFriction) {
		this.movementFriction = movementFriction;
	}
	public double getRotationFriction() {
		return rotationFriction;
	}
	public void setRotationFriction(double rotationFriction) {
		this.rotationFriction = rotationFriction;
	}
	public double getDistance(){
		return distance;
	}
	
}